#include <math.h>
//#include <device.h>

#include "fdecl.h"
#include "misc.h"

void zeroize_force(hybroid *h)
{
  int i;
  vector *f;

  for (i = 0; i < h->numnodes; i++) {
    f = &h->nodes[i].force;
    vzeroize(*f); 
  }
}


void unrotate_force(hybroid *h)
{
  int i;
  node *ndei, *nds = h->nodes;
  vector *f, *rf;
  quaternion inverse_q;
  double inverse_rotation_matrix[3][3];
  
  inverse_q.w =  - (h->orientation.w);
  vset(h->orientation.v, inverse_q.v);
  //Mingchen get rotation matric inverse_rotation_matrix from quaternion inverse_q
  unit_quaternion_to_rotation_matrix(inverse_q, inverse_rotation_matrix);

  for (i = 0; i < h->numnodes; i++) {
    ndei = &nds[i];
    f = &ndei->force;
    rf = &ndei->rforce;
    matvecmult(inverse_rotation_matrix, *f, *rf);
  }
}

void compute_net_applied_force_and_torque(hybroid *h)
{
  int i;
  node *nde;
  vector *p, *f, r, temp, *t = &h->translation;
  vector *nf = &h->net_applied_force, *nt = &h->net_applied_torque;

  vzeroize(*nf); vzeroize(*nt);
	
	//Mingchen
	//r = posit-translation
	//nf = net_applied_force + force
	//temp = r cross force
	//nt = nt + temp

  for (i = 0; i < h->numnodes; i++) {
      nde = &h->nodes[i];
      p = &nde->posit;
      vminus(*p, *t, r);

      f = &nde->force;
      vinc(*f, *nf);

      vcross(r, *f, temp);
      vinc(temp, *nt);      
  }
}


void compute_net_damping_force_and_torque(hybroid *h)
{
  int i;
  node *nde;
  vector *p, *v, df, r, temp, *t = &h->translation;
  vector *nf = &h->net_damping_force, *nt = &h->net_damping_torque;
  double d = h->damp;

  vzeroize(*nf); vzeroize(*nt);

  for (i = 0; i < h->numnodes; i++) {
      nde = &h->nodes[i];
      p = &nde->posit;
      vminus(*p, *t, r);

      v = &nde->veloc;
      vscale(d, *v, df);
      
      vinc(df, *nf);

      vcross(r, df, temp);
      vinc(temp, *nt);      
  }
}

void nodes_in_patch
  (hybroid *h, double u, double v,
   node **n00, node **n01, node **n10, node **n11)
{
  double i0 = floor((u - h->ubegin) / h->ustep);
  double j0 = floor((v - h->vbegin) / h->vstep);
  int k00, k01, k10, k11;
  int vsz = h->vsize;
  node *nds = h->nodes;

  k00 = (int)i0 * vsz + (int)j0;
  k10 = k00 + vsz;
  k01 = k00 + 1;
  k11 = k10 + 1;

  if (j0 < 0) {
    k00 = k10 + vsz;
    k10 = k11 + vsz;
  }
  if (j0 >= vsz - 1) {
    k01 = k00 - vsz;
    k11 = k10 - vsz;
  }
  if (i0 < 0) {
    k00 = h->numnodes - 2;
    k01 = k00;
  }
  if (i0 >= h->usize - 1) {
    k10 = h->numnodes - 1;
    k11 = k10;
  }

  *n00 = &nds[k00];
  *n01 = &nds[k01];
  *n10 = &nds[k10];
  *n11 = &nds[k11];
}

void distribute_force_on_nodes
  (vector *force, double s, double t, double h, double k,
   node *n00, node *n01, node *n10, node *n11)
{
  double hk = h * k;
  vector f;
  
  vscale1((h - s) * (k - t) / hk, *force, f);
  vinc(f, n00->force);

  vscale1(s * (k - t) / hk, *force, f);
  vinc(f, n10->force);

  vscale1(s * t / hk, *force, f);
  vinc(f, n11->force);

  vscale1((h - s) * t / hk, *force, f);
  vinc(f, n01->force);
}



void compute_bilinear_patch_tangent_vectors
    (double s, double t, double h, double k,
     vector *p00, vector *p01, vector *p10, vector *p11,
     vector *fu, vector *fv)
{
  double hs = h - s, kt = k - t, hk = h * k;
  vector p00p, p10p, p01p, p11p, fst;

  vscale1(- kt / hk, *p00, p00p);
  vscale1(  kt / hk, *p10, p10p);
  vscale1(   t / hk, *p11, p11p);
  vscale1(-  t / hk, *p01, p01p);
  vplus(p00p, p10p, p00p);
  vplus(p11p, p01p, p11p);
  vplus(p00p, p11p, *fu);
  vnormalize(*fu);

  vscale1(- hs / hk, *p00, p00p);
  vscale1(-  s / hk, *p10, p10p);
  vscale1(   s / hk, *p11, p11p);
  vscale1(  hs / hk, *p01, p01p);
  vplus(p00p, p10p, p00p);
  vplus(p11p, p01p, p11p);
  vplus(p00p, p11p, *fv);
  vnormalize(*fv);
}


double ASTEP = 1.0;

void adjust_datapoint_uv_attachment
  (range_datapoint *datapoint, vector *force,
   double s, double t, double h, double k,
   node *n00, node *n01, node *n10, node *n11)
{
  double pi = 3.141592653589, pi2 = 2.0 * pi, pi_2 = pi / 2.0;
  vector fu, fv;
  double u, v, du, dv;

  compute_bilinear_patch_tangent_vectors(s, t, h, k,
					 &n00->posit, &n01->posit, 
					 &n10->posit, &n11->posit, &fu, &fv);
/*
  {
    vector p;
    bilinear_patch(s, t, h, k,
		 &n00->posit, &n01->posit, &n10->posit, &n11->posit, &p);
    draw_vector(&fu, &p, .2, YELLOW);
    draw_vector(&fv, &p, .2, YELLOW);
  }
*/

  du = ASTEP * vdot(*force, fu);
  dv = ASTEP * vdot(*force, fv);


  {
    vector p, f, g;
    bilinear_patch(s, t, h, k,
		 &n00->posit, &n01->posit, &n10->posit, &n11->posit, &p);
    vscale(du, fu, f);
    //draw_vector(&f, &p, 40.0, YELLOW);
    vscale(dv, fv, g);
    //draw_vector(&g, &p, 40.0, YELLOW);
  }


  du = Limit(du, -h, h);
  dv = Limit(dv, -k, k);

  u = datapoint->u; v = datapoint->v;


/*
  u += du; v += dv;
  u = Limit(u, -pi_2, pi_2);
  v = Limit(v, -pi, pi - h);
*/


  u += du; 
  u = Limit(u, -pi_2, pi_2);

  v += dv;
  if (v >  pi-h) v = - pi;
  if (v < -pi) v = pi;


/*
  u += du; 
  if (u >    pi_2) {u =  pi - u; v = v + pi;}
  if (u <= - pi_2) {u = -pi - u; v = v + pi;}

  v += dv;
  if (v >  pi - h) v = v - pi2;
  if (v < -pi    ) v = v + pi2;
*/

  datapoint->u = u; datapoint->v = v;
}

void compute_range_data_forces
  (hybroid *h)
{
  int i;
  double u, v, s, t, distance;
  node *nearest_node;
  range_datapoint *datapoint;
  vector p, separation;
  double hh = h->ustep, kk = h->vstep;
  node *n00, *n01, *n10, *n11;
  node *nn00, *nn01, *nn10, *nn11;

  for (i = 0, datapoint = h->range_data; i < h->numdatapoints; i++,
       datapoint++) {
    u = datapoint->u; v = datapoint->v;

    nodes_in_patch(h, u, v, &n00, &n01, &n10, &n11);
    
/*
    draw_line_between_points(&n00->posit, &n11->posit, RED);
    draw_line_between_points(&n10->posit, &n01->posit, BLUE);

    nodes_in_patch(h, u-hh, v+kk, &nn00, &nn01, &nn10, &nn11);

    draw_line_between_points(&nn00->posit, &nn11->posit, GREEN);
    draw_line_between_points(&nn10->posit, &nn01->posit, BLUE);
*/

    s = u - n00->uu; t = v - n00->vv;
    bilinear_patch(s, t, hh, kk,
		   &n00->posit, &n01->posit, &n10->posit, &n11->posit, &p);
       
    //color(MAGENTA);
    //bgnline();
    //v3d((double *)&p);
    //v3d((double *)&datapoint->position);
    //endline();
    //color(CYAN);

    vminus(datapoint->position, p, separation);
    distance = vlength(separation);
    vscale(datapoint->strength_force, separation, separation);

    distribute_force_on_nodes(&separation, s, t, hh, kk, 
			      n00, n01, n10, n11);
    adjust_datapoint_uv_attachment(datapoint, &separation, s, t, hh, kk,
				   n00, n01, n10, n11);
  }
}


void compute_range_data_forces_exhaustive(hybroid *h)
{
  int i;
  double d;
  range_datapoint *datapoint;
  vector separation;
  int find_nearest_nodes = 0;
/*
  static find_nearest_nodes = 0;
*/
  
  find_nearest_nodes = find_nearest_nodes % 200;

  //Mingchen separation = strength_force*(position-nearest_node.posit)
  //set datapoint->nearest_node->force;

  for (i = 0, datapoint = h->range_data; i < h->numdatapoints; i++,
       datapoint++) {
    if (find_nearest_nodes == 0) 
      datapoint->nearest_node = nearest_hybroid_node(h, &datapoint->position,
						     &separation, &d);
    vminus(datapoint->position, datapoint->nearest_node->posit, separation);
    vscale(datapoint->strength_force, separation, separation);
    vinc(separation, datapoint->nearest_node->force);
  }
  find_nearest_nodes++;
}


hybroid *nearest_hybroid
  (hybroid **hybroids, int num_hybroids, vector *point, 
   vector *separation, double *distance)
{
  int i;
  hybroid *hyb, *nearest_hybroid;
  vector sep;
  double dist_sq;

  *distance = 10.0e40;

  for (i = 0; i < num_hybroids; i++) {
    hyb = hybroids[i];
    vminus(*point, hyb->translation, sep);
    dist_sq = sep.x * sep.x + sep.y * sep.y + sep.z * sep.z;
    if (dist_sq < *distance) {
      *distance = dist_sq;
      vset(sep, *separation);
      nearest_hybroid = hyb;
    }
  }
  *distance = (double)sqrtf((float)dist_sq);
  return nearest_hybroid;
}


node *nearest_hybroid_node(hybroid *h, vector *point, 
			   vector *separation, double *distance)
{
  int i;
  node *nde, *nearest_node;
  vector sep;
  double dist_sq;

  *distance = 10.0e40;

  //Mingchen: go through all the nodes of hybroid h, calculate the sep
  //set separation and the distance
  //return the nearest node

  for (i = 0; i < h->numnodes; i++) {
    nde = &h->nodes[i];
    vminus(*point, nde->posit, sep);
    dist_sq = sep.x * sep.x + sep.y * sep.y + sep.z * sep.z;
    if (dist_sq < *distance) {
      *distance = dist_sq;
      vset(sep, *separation);
      nearest_node = nde;
    }
  }
  *distance = (double)sqrtf((float)dist_sq);
  return(nearest_node);
}


void bat_spring_force(vector *bat_position, hybroid *h, double strength)
{
  int i;
  node *bat_node;
  double d;
  vector separation;
  
  bat_node = nearest_hybroid_node(h, bat_position, &separation, &d);
  vscale(strength, separation, separation);
  vinc(separation, bat_node->force);
  //display_bat_spring(bat_position, &bat_node->posit);
}


void tabletop_forces
  (hybroid *h, double tabletop_z_position, double strength)
{
  int i;
  node *nde, *nds = h->nodes;
  double d;

  for (i = 0; i < h->numnodes; i++) {
    nde = &nds[i];
    d = nde->posit.z - tabletop_z_position;
    if (d < 0.0) nde->force.z -= strength * d;
  }
}


void limit_external_forces
  (hybroid *h, double force_mag_limit)
{
  int i;
  node *nds = h->nodes;
  vector *f;
  double m, force_mag_limit_sq = force_mag_limit * force_mag_limit;

	//Mingchen limit the length of force to force_mag_limit

  for (i = 0; i < h->numnodes; i++) {
    f = &nds[i].force;
    m = f->x * f->x + f->y * f->y + f->z * f->z;
    if (m > force_mag_limit_sq) { 
      vscale(force_mag_limit / (double)sqrtf((float)m), *f, *f);
    }
  }
	//Mingchen print node forces
	//printf("node forces\n");
	//for (i = 0; i < h->numnodes; i++) {
	//	f = &nds[i].force;
	//	printf("%f,%f,%f\n",f->x,f->y,f->z);
	//}
}


void image_interp_grad
  (double x, double y, int *image, int xdim, int ydim,
   double *df_dx, double *df_dy)
{
  int x0, x1, y0, y1;
  double xx, yy, f00, f10, f01, f11;

  if (x >= 0.0 && x < xdim-1 && y >= 0.0 && y < ydim-1) {
    x0 = (int)x; y0 = (int)y;
    x1 = x0 + 1; y1 = y0 + 1;
    xx = x - x0; yy = y - y0;
    f00 = (double)image[x0*ydim+y0];
    f10 = (double)image[x1*ydim+y0];
    f01 = (double)image[x0*ydim+y1];
    f11 = (double)image[x1*ydim+y1];
    *df_dx = (yy * (f11 - f01) + (1.0 - yy) * (f10 - f00));
    *df_dy = (xx * (f11 - f10) + (1.0 - xx) * (f01 - f00));
  }
  else {
    *df_dx = 0.0;
    *df_dy = 0.0;
  }
}


void compute_image_forces
  (hybroid *h, double z_threshold)
{
  int i;
  node *nde, *nds = h->nodes;
  vector *p, *f;
  double dx, dy, xs = h->image_xsize, ys = h->image_ysize, scale = Max(xs, ys);
  double strength = h->strength_image_force;
  int *image = h->image;

  for (i = 0; i < h->numnodes; i++) {
    nde = &nds[i];
    p = &nde->posit;
    f = &nde->force;
    if (fabs(p->z) < z_threshold) {
      image_interp_grad(p->x * scale, p->y * scale, image, xs, ys, &dx, &dy);
      f->x += dx * strength;
      f->y += dy * strength;
    }
  }
}


void mouse_spring_force
  (vector *mouse_position, node *nearest_node, double strength)
{
  vector separation;
  
  vminus(*mouse_position, nearest_node->posit, separation);
  vscale(strength, separation, separation);
  separation.z = 0.0;		/* cancel spring force z component */
  vinc(separation, nearest_node->force);
}



